// Required App Version: 0.7.0
// ---------------------------------------------------------------------------
// This Arduino Nano sketch accompanies the OpenBot Android application.
//
// The sketch has the following functionalities:
//  - receive control commands and sensor config from Android application (USB serial)
//  - produce low-level controls (PWM) for the vehicle
//  - toggle left and right indicator signals
//  - wheel odometry based on optical speed sensors
//  - estimate battery voltage via voltage divider
//  - estimate distance based on sonar sensor
//  - control LEDs for status and at front and back of vehicle
//  - send sensor readings to Android application (USB serial)
//  - display vehicle status on OLED
//
// Dependencies: Install via "Tools --> Manage Libraries" (type library name in the search field)
//  - Interrupts: PinChangeInterrupt by Nico Hood (read speed sensors and sonar)
//  - OLED: Adafruit_SSD1306 & Adafruit_GFX (display vehicle status)
//  - Servo: Built-In library by Michael Margolis (required for RC truck)
// Contributors:
//  - October 2020: OLED display support by Ingmar Stapel
//  - December 2021: RC truck support by Usman Fiaz
//  - March 2022: OpenBot-Lite support by William Tan
//  - May 2022: MTV support by Quentin Leboutet
//  - Jan 2023: BLE support by iTinker
// ---------------------------------------------------------------------------

// By Matthias Mueller, 2023
// ---------------------------------------------------------------------------

#include "Config.h"
#include "Motor.h"
#include "AHT20.h"
#include "IRremote.h"

// Enable/Disable coast mode (1,0)
// When no control is applied, the robot will either coast (1) or actively stop (0)
boolean coast_mode = 1;

char header = '\n';
int bodyvalue = 0;
int bodyvalue2 = -1;   //例如温湿度,需要用2个变量接收
char endChar = '\n';
const char MAX_MSG_SZ = 60;
char msg_buf[MAX_MSG_SZ] = "";
int msg_idx = 0;

//------------------------------------------------------//
// INITIALIZATION
//------------------------------------------------------//
#if (NO_PHONE_MODE)
unsigned long turn_direction_time = 0;
unsigned long turn_direction_interval = 5000;
unsigned int turn_direction = 0;
int ctrl_max = 192;
int ctrl_slow = 96;
int ctrl_min = (int)255.0 * VOLTAGE_MIN / VOLTAGE_MAX;
#endif

#if (HAS_SONAR)
#if (MCU == NANO)
#include "PinChangeInterrupt.h"
#endif
// Sonar sensor
const float US_TO_CM = 0.01715;              // cm/uS -> (343 * 100 / 1000000) / 2;
const unsigned int MAX_SONAR_DISTANCE = 300; // cm
const unsigned long MAX_SONAR_TIME = (long)MAX_SONAR_DISTANCE * 2 * 10 / 343 + 1;
const unsigned int STOP_DISTANCE = 10; // cm
#if (NO_PHONE_MODE)
const unsigned int TURN_DISTANCE = 50;
unsigned long sonar_interval = 100;     // 超声波传感器测量的间隔时间
#else
unsigned long sonar_interval = 1000;
#endif
unsigned long sonar_time = 0;
boolean sonar_sent = false;
boolean ping_success = false;
unsigned int distance = -1;          // cm
unsigned int distance_estimate = -1; // cm
unsigned long start_time;
unsigned long echo_time = 0;
#if (SONAR_MEDIAN)
const unsigned int distance_array_sz = 3;
int distance_array[distance_array_sz] = {};
unsigned int distance_counter = 0;
#endif
#else
const unsigned int TURN_DISTANCE = -1; // cm
const unsigned int STOP_DISTANCE = 0;  // cm
unsigned int distance_estimate = -1;   // cm
#endif

#if (HAS_OLED)
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

const int OLED_RESET = -1; // not used
Adafruit_SSD1306 display(OLED_RESET);

// OLED Display SSD1306
const unsigned int SCREEN_WIDTH = 128; // OLED display width, in pixels
const unsigned int SCREEN_HEIGHT = 32; // OLED display height, in pixels
#endif

#if (HAS_TEMPERATURE_HUMIDITY)
AHT20 aht20;
#endif

IRsend irsend;

// 控制速度变量
int ctrl_left = 0;
int ctrl_right = 0;
int ctrl_forward = 0;
int ctrl_backward = 0;
int ctrl_turnleft = 0;
int ctrl_turnright = 0;

// 控制转向灯状态
#if (HAS_INDICATORS)
char indicator_light = 0;  // 00: 关闭 10: 左转 01: 右转 11: 双灯
#endif

//////////////////////////////////////////////////////////////////////////
//--------------------刷新时间变量定义-------------------------------------
#if (HAS_VOLTAGE_DIVIDER)
// Voltage measurement
unsigned int vin_counter = 0;
const unsigned int vin_array_sz = 10;
int vin_array[vin_array_sz] = {0};

unsigned long voltage_interval = 10000; // 电池电压测量的间隔时间
unsigned long voltage_time = 0;
#endif

#if (HAS_TEMPERATURE_HUMIDITY)
unsigned long temperature_interval = 10000; // 温湿度发送间隔10s
unsigned long temperature_time = 0;
#endif

// Heartbeat
unsigned long heartbeat_interval = -1;
unsigned long heartbeat_time = 0;

#if (HAS_OLED || DEBUG)
// Display (via Serial)
unsigned long display_interval = 1000; // How frequently vehicle data is displayed (ms).
unsigned long display_time = 0;
#endif

//////////////////////////////////////////////////////////////////////////

// 发送红外命令 - IRremote
void sendIRCommand(uint32_t sRawData) {
  Serial.print("发送红外命令: 0x");
  Serial.println(sRawData, HEX);

  irsend.sendSony(0xa90, 12);

  delay(50); 
}

#if HAS_BODYSENSE
void bodysense_isr();
#endif

//------------------------------------------------------//
// SETUP
//------------------------------------------------------//
void setup()
{
#if (MCU == NANO)
  pinMode(PIN_PWM_L1, OUTPUT);
  pinMode(PIN_PWM_L2, OUTPUT);
  pinMode(PIN_PWM_R1, OUTPUT);
  pinMode(PIN_PWM_R2, OUTPUT);
  // 添加负极引脚
  pinMode(PIN_L1_B, OUTPUT);
  pinMode(PIN_L2_B, OUTPUT);
  pinMode(PIN_R1_B, OUTPUT);
  pinMode(PIN_R2_B, OUTPUT);
#endif
  // Initialize with the I2C addr 0x3C
#if (HAS_INDICATORS)
  pinMode(PIN_LED_LI, OUTPUT);
  pinMode(PIN_LED_RI, OUTPUT);
#endif
  // Test sequence for indicator LEDs
#if (HAS_INDICATORS)
  digitalWrite(PIN_LED_LI, LOW);
  digitalWrite(PIN_LED_RI, LOW);
  delay(500);
  digitalWrite(PIN_LED_LI, HIGH);
  delay(500);
  digitalWrite(PIN_LED_LI, LOW);
  digitalWrite(PIN_LED_RI, HIGH);
  delay(500);
  digitalWrite(PIN_LED_RI, LOW);
#endif
#if (HAS_SONAR)
  pinMode(PIN_ECHO, INPUT);
  pinMode(PIN_TRIGGER, OUTPUT);
#endif
#if (HAS_VOLTAGE_DIVIDER)
  pinMode(PIN_VIN, INPUT);
#endif
#if (HAS_BUMPER)
  pinMode(PIN_BUMPER, INPUT);
#endif
#if (HAS_BODYSENSE)
  pinMode(PIN_BODYSENSE, INPUT);
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PIN_BODYSENSE), bodysense_isr, CHANGE);
#endif

  /*/红外初始化
  IrSender.begin(IR_SEND_PIN);
  IrSender.enableIROut(38);
  delay(20);*/

  #if (DEBUG)
    /*Serial.print(F("Send signal mark duration for 38kHz is "));
    Serial.print(IrSender.periodOnTimeMicros);
    Serial.print(F(" us, pulse narrowing correction is "));
    Serial.print(IrSender.getPulseCorrectionNanos());
    Serial.print(F(" ns, total period is "));
    Serial.print(IrSender.periodTimeMicros);
    Serial.println(F(" us"));*/
  #endif

  Serial.begin(115200, SERIAL_8N1);
  // SERIAL_8E1 - 8 data bits, even parity, 1 stop bit
  // SERIAL_8O1 - 8 data bits, odd parity, 1 stop bit
  // SERIAL_8N1 - 8 data bits, no parity, 1 stop bit
  // Serial.setTimeout(10);
  Serial.println('r');
  Serial.println("RemoteCar is ready.");

#if (HAS_TEMPERATURE_HUMIDITY)
  Wire.begin();
  if (aht20.begin() == false)
  {
    Serial.println("AHT20 not detected. Please check wiring. Freezing.");
    //while(true);
  }
#endif
}

//------------------------------------------------------//
// FUNCTIONS 读取电池电压
//------------------------------------------------------//
#if HAS_VOLTAGE_DIVIDER

float get_voltage()
{
  unsigned long array_sum = 0;
  unsigned int array_size = min(vin_array_sz, vin_counter);
  for (unsigned int index = 0; index < array_size; index++)
  {
    array_sum += vin_array[index];
  }
  return float(array_sum) / array_size * ADC_FACTOR * VOLTAGE_DIVIDER_FACTOR;
}

#endif

//------------------------------------------------------//
// FUNCTIONS 读取温湿度
//------------------------------------------------------//
#if HAS_TEMPERATURE_HUMIDITY
void get_temperature(int* temp, int* humid)
{
  float t = aht20.getTemperature();
  float h = aht20.getHumidity();

  *temp = (int)(t + 0.5);
  *humid = (int)(h + 0.5);
}
#endif

void update_left_motors()
{
  if (ctrl_left > 0)
  {
    motor.goLeft(ctrl_left);
  }
  else
  {
    motor.stopRobot();
  }
}

void update_right_motors()
{
  if (ctrl_right > 0)
  {
    motor.goRight(ctrl_right);
  }
  else
  {
    motor.stopRobot();
  }
}

void update_forward_motors()
{
  if (ctrl_forward > 0)
    motor.goAhead(ctrl_forward);
  else
    motor.stopRobot();
}

void update_backward_motors()
{
  if (ctrl_backward > 0)
    motor.goBack(ctrl_backward);
  else
    motor.stopRobot();
}

void update_turnleft_motors()
{
  if (ctrl_turnleft > 0)
    motor.turnLeft(ctrl_turnleft);
  else
    motor.stopRobot();
}

void update_turnright_motors()
{
  if (ctrl_turnright > 0)
    motor.turnRight(ctrl_turnright);
  else
    motor.stopRobot();
}

void update_vehicle()
{
  update_left_motors();
  update_right_motors();
  update_forward_motors();
  update_backward_motors();
  update_turnleft_motors();
  update_turnright_motors();
}

//每次收到运动命令后持续运行的时间设置
void update_heartbeat(int maitain_time)
{
  heartbeat_interval = maitain_time;
  heartbeat_time = millis();
}

boolean almost_equal(int a, int b, int eps)
{
  return abs(a - b) <= eps;
}


void sendData(String data)
{
  Serial.print(data);
  Serial.println();
}

/*
发送消息格式：
@A,255,10\n 前进全速,10为持续时间
@B,128,5\n 后退半速
@L,128,10\n 半速向左
@R,128,20\n 半速向右
@U,128,10\n 左转圈半速
@V,128,20\n 右转圈半速
@C,0\n 停止
@M,1,0\n  开关灯,1为左灯 2为右灯, 第二个数字0为关, 1为开
@F,A1A2A3A4\n 红外发射数据
@K,0\n  查询充电状态

返回消息格式：
@Q,80\n 电池电量
@T,25,60\n 温湿度数据
@P,1\n 人体感应数据,时间戳由MQTT发送时间确定
@S,100\n 超声波检测距离数据
@W,100\n  轮子速度数据
@K,1\n  充电状态，0为未充电或充满，1为正在充电
*/

// 处理接收到的消息
void parse_msg()
{
  int len = strlen(msg_buf);
  // 检查格式：以@开头，以\n结尾，至少有4个字符（@A,\n）
  if (len < 4 || msg_buf[0] != '@' || msg_buf[len - 1] != '\n')
  {
    return; // 格式不符合要求
  }
  // 提取命令字符（@后的第一个字符）
  header = msg_buf[1];

  char *token = strtok(msg_buf, ",\n"); // replace delimiter with \0
  if (!token)
  {
    return; // 格式不符合要求
  }
  token = strtok(NULL, ",\n");
  if (token)
  {
    bodyvalue = atoi(token);
    token = strtok(NULL, ",\n");   //有的命令需要2个数据,在这里接收第二个数据
		if (token) {
			bodyvalue2 = atoi(token);
		}
  }
  
  switch (header)
  {
    case 'A': ctrl_forward = bodyvalue;  update_heartbeat(bodyvalue2); break;   // 前进
    case 'B': ctrl_backward = bodyvalue; update_heartbeat(bodyvalue2); break;   // 后退
    case 'L': ctrl_left = bodyvalue; update_heartbeat(bodyvalue2); break;       // 左转
    case 'R': ctrl_right = bodyvalue; update_heartbeat(bodyvalue2); break;      // 右转
    case 'U': ctrl_turnleft = bodyvalue; update_heartbeat(bodyvalue2); break;   // 左转圈
    case 'V': ctrl_turnright = bodyvalue; update_heartbeat(bodyvalue2); break;  // 右转圈
    case 'C': ctrl_forward = 0; ctrl_backward = 0; ctrl_left = 0; ctrl_right = 0;  break; // 停止
    case 'M': {  // 设置指示灯
      #if (HAS_INDICATORS)
      indicator_light = bodyvalue;
      bool is_on = (bodyvalue2 == 1);
      if(indicator_light == 1){
        if(is_on)   digitalWrite(PIN_LED_LI, HIGH);
        else        digitalWrite(PIN_LED_LI, LOW);
      }
      else if(indicator_light == 2){
        if(is_on)   digitalWrite(PIN_LED_RI, HIGH);
        else        digitalWrite(PIN_LED_RI, LOW);
      }
      #endif
    } break;
    case 'F': {  // 红外发射
      sendIRCommand(strtoul(msg_buf + 4, NULL, 16));
    } break;
    case 'K': {  // 查询充电状态
      int charge_status = 0;
      sendData("@K," + String(charge_status));
    } break;
    default:
      break;
  }
#if DEBUG
  Serial.print("Control: ");
  Serial.print(header);
  Serial.print(" = ");
  Serial.println(bodyvalue);
#endif

  header = '\0';
  bodyvalue = 0;
}

// 处理串口消息
void on_serial_rx()
{
  char inChar = Serial.read();
  if (inChar != endChar)
  {
    msg_buf[msg_idx] = inChar;
    msg_idx++;
  }
  else
  {
    msg_buf[msg_idx] = '\0'; // end of message
    parse_msg();             // 处理BODY数据
    msg_idx = 0;
  }
}

int get_median(int a[], int sz)
{
  // bubble sort
  for (int i = 0; i < (sz - 1); i++)
  {
    for (int j = 0; j < (sz - (i + 1)); j++)
    {
      if (a[j] > a[j + 1])
      {
        int t = a[j];
        a[j] = a[j + 1];
        a[j + 1] = t;
      }
    }
  }
  return a[sz / 2];
}

//------------------------------------------------------//
// INTERRUPT SERVICE ROUTINES (ISR)  中断服务程序, 用于人体感应
//------------------------------------------------------//
#if HAS_BODYSENSE
void bodysense_isr()
{
    sendData("@P,1");
}
#endif

//------------------------------------------------------//
// INTERRUPT SERVICE ROUTINES (ISR)  中断服务程序, 用于超声波传感器
//------------------------------------------------------//
#if HAS_SONAR
// ISR: Stop timer and record the time
void stop_timer()
{
  echo_time = micros() - start_time;
  detachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PIN_ECHO));
  ping_success = true;
}

// ISR: Start timer to measure the time it takes for the pulse to return
void start_timer()
{
  start_time = micros();
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PIN_ECHO), stop_timer, FALLING);
}
//-----------------------------------------------------//

void send_sonar_reading() {
  //只在查询时发送
  //sendData("@S," + String(distance_estimate));
}

// Send pulse by toggling trigger pin
void send_ping()
{
  echo_time = 0;
  ping_success = false;
  if (PIN_TRIGGER == PIN_ECHO)
    pinMode(PIN_TRIGGER, OUTPUT);

  digitalWrite(PIN_TRIGGER, LOW);
  delayMicroseconds(5);
  digitalWrite(PIN_TRIGGER, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIGGER, LOW);

  if (PIN_TRIGGER == PIN_ECHO)
    pinMode(PIN_ECHO, INPUT);

  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(PIN_ECHO), start_timer, RISING);
}

void update_distance_estimate()
{
#if SONAR_MEDIAN
  distance_array[distance_counter % distance_array_sz] = distance;
  distance_counter++;
  distance_estimate = get_median(distance_array, distance_array_sz);
#else
  distance_estimate = distance;
#endif
}

#endif   // end of #if HAS_SONAR

//------------------------------------------------------//
// LOOP
//------------------------------------------------------//
void loop()
{
  if (Serial.available() > 0)
  {
    on_serial_rx();
  }

  Serial.println("ctrl_left = " + String(ctrl_left));
  Serial.println("ctrl_right = " + String(ctrl_right));
  Serial.println("ctrl_forward = " + String(ctrl_forward));
  Serial.println("ctrl_backward = " + String(ctrl_backward));
  Serial.println("ctrl_turnleft = " + String(ctrl_turnleft));
  Serial.println("ctrl_turnright = " + String(ctrl_turnright));

  delay(300);
  return;
  
  // 如果预估碰撞距离小于安全距离,则小车停止
  if (distance_estimate <= STOP_DISTANCE && ctrl_left > 0 && ctrl_right > 0 && ctrl_forward > 0 && ctrl_backward > 0)
  {
    ctrl_left = 0;
    ctrl_right = 0;
    ctrl_forward = 0;
    ctrl_backward = 0;
    ctrl_turnleft = 0;
    ctrl_turnright = 0;
  }

  // 如果超过心跳时间,则小车停止
  // 每次发送运动命令时，必须先发送心跳时间,决定本次运动执行的最长时间
  if ((millis() - heartbeat_time) >= heartbeat_interval)
  {
    ctrl_left = 0;
    ctrl_right = 0;
    ctrl_forward = 0;
    ctrl_backward = 0;
    ctrl_turnleft = 0;
    ctrl_turnright = 0;
  }

  update_vehicle();

#if HAS_VOLTAGE_DIVIDER
  // Measure voltage
  vin_array[vin_counter % vin_array_sz] = analogRead(PIN_VIN);
  vin_counter++;
#endif

#if HAS_SONAR
  // Check for successful sonar reading
  if (!sonar_sent && ping_success)
  {
    distance = echo_time * US_TO_CM;
    update_distance_estimate();
    send_sonar_reading();
    sonar_sent = true;
  }
  // Measure distance every sonar_interval
  if ((millis() - sonar_time) >= max(sonar_interval, MAX_SONAR_TIME))
  {
    if (!sonar_sent && !ping_success)
    { // Send max val if last ping was not returned
      distance = MAX_SONAR_DISTANCE;
      update_distance_estimate();
      send_sonar_reading();
      sonar_sent = true;
    }
    sonar_time = millis();
    sonar_sent = false;
    send_ping();
  }
#endif

#if HAS_VOLTAGE_DIVIDER
  // Send voltage reading via serial
  if ((millis() - voltage_time) >= voltage_interval)
  {
    sendData("@Q," + String(get_voltage(), 10));
    voltage_time = millis();
  }
#endif

#if HAS_TEMPERATURE_HUMIDITY
  if((millis() - temperature_time) >= temperature_interval)
  {
    int temper, humid;
    get_temperature(&temper, &humid);
    sendData("@T," + String(temper, 10) + "," + String(humid, 10));
    temperature_time = millis();

    #if DEBUG
    Serial.print("Termperature= ");
    Serial.print(temper);
    Serial.print("Humidity= ");
    Serial.println(humid);
    #endif
  } 
#endif
}
